#include "e2e_processor.h"

#include <filesystem>

#include "common/basic_settings.h"
#include "common/file_helper.h"
#include "common/utility.h"
#include "data_adapter.h"
#include "visualization/perception_visualizer.h"

namespace autodrive {
namespace perception {
E2EProcessor::E2EProcessor()
    : is_init_{false},
      is_run_{false},
      cmd_stop_{false},
      result_buffer_ptr_{nullptr} {}

E2EProcessor::~E2EProcessor() { Stop(); }

E2EProcessor& E2EProcessor::GetInstance() noexcept {
  static E2EProcessor instance;
  return instance;
}

/// @brief init
void E2EProcessor::Init() noexcept(false) {
  auto initFunc{[this]() {
    std::string const& configFileName{"E2EProcessorSettings.json"};
    auto const& configFile{std::filesystem::path(sConfigRootPath) /
                           std::filesystem::path(configFileName)};
    if (!std::filesystem::exists(sConfigRootPath)) {
      if (!std::filesystem::create_directories(sConfigRootPath)) {
        throw std::runtime_error("create directory " + sConfigRootPath +
                                 " fail");
      }
    }

    if (!std::filesystem::exists(configFile)) {
      // use default settings
      auto const& defaultSettings{gen_default()};
      auto const& sjSettings{serialize_json(defaultSettings)};
      write_json(configFile.string(), sjSettings);
    }

    auto const& sSettings{read_text(configFile)};
    settings_ = json_deserialize<E2EProcessorSettings>(sSettings);

    auto const& eSettings{settings_.e2e_net_settings};
    e2e_net_.Init(eSettings);
  }};

  std::call_once(init_flag_, initFunc);
  is_init_ = true;
}

void E2EProcessor::Start() noexcept(false) {
  auto runFunc{[&]() { task_thread_ = std::thread{main_task, this}; }};
  std::call_once(run_flag_, runFunc);
  is_run_ = true;
}

E2EProcessor::E2EProcessorSettings E2EProcessor::gen_default() const noexcept {
  return E2EProcessorSettings{
      .e2e_net_settings{.trt_infer_settings{.model_file{"model.onnx"},
                                            .use_cuda_device{0},
                                            .use_dla_core{-1},
                                            .max_batch_size{1},
                                            .max_work_space_size{4},
                                            .scaler_type{"fp16"}}}};
}

void E2EProcessor::main_task(E2EProcessor* e2eProcessorPtr) noexcept {
  try {
    if (nullptr == e2eProcessorPtr) {
      throw std::runtime_error("e2eProcessorPtr is nullptr");
    }

    if (nullptr == e2eProcessorPtr->result_buffer_ptr_) {
      e2eProcessorPtr->result_buffer_ptr_ =
          std::make_shared<PerceptionResultBuffer>();
    }

    // visualization
    auto& vis{PerceptionVisualizer::GetInstance()};
    vis.Init();

    auto const& settings(e2eProcessorPtr->Settings());

    std::uint64_t currentTimestamp{0};

    while (true) {
      // command to stop task
      if (e2eProcessorPtr->cmd_stop_) {
        std::cout << "[" << to_yellow("Warr") << "]"
                  << " E2EProcessor command to stop ..." << std::endl;
        break;
      }
      auto& dataAdapter{autodrive::perception::DataAdapter::GetInstance()};

      auto imageSouceBufferPtr{dataAdapter.GetImageSourceBufferPtr()};
      if (nullptr == imageSouceBufferPtr) {
        std::cout << "[" << to_yellow("Warr") << "]"
                  << " Wait image source buffer ready ..." << std::endl;
        continue;
      }

      auto pclBufferPtr{dataAdapter.GetPointCloudBufferPtr()};
      if (nullptr == pclBufferPtr) {
        std::cout << "[" << to_yellow("Warr") << "]"
                  << " point cloud source buffer is not ready ..." << std::endl;
      }

      try {
        // image data
        if (imageSouceBufferPtr->IsEmpty()) {
          std::cout << "[" << to_yellow("Warr") << "]"
                    << "E2EProcessor waiting image data ......" << std::endl;
          e2eProcessorPtr->msleep(50);
          continue;
        }

        auto imageSourcePtr{imageSouceBufferPtr->GetLatest().second};
        PerceptionResult* resultPtr{nullptr};
        if (nullptr == imageSourcePtr ||
            imageSourcePtr->timestamp == currentTimestamp) {
          std::cout << "[" << to_yellow("Warr") << "]"
                    << "E2EProcessor waiting image data update......"
                    << std::endl;
          e2eProcessorPtr->msleep(50);
          continue;
        } else {
          currentTimestamp = imageSourcePtr->timestamp;
          resultPtr = e2eProcessorPtr->result_buffer_ptr_->GetNext();
        }

        if (nullptr == resultPtr) {
          throw std::runtime_error("E2EProcessor result is nullptr");
        }

        std::cout << "[" << to_green("Info") << "]"
                  << "Receive image data: " << currentTimestamp << std::endl;

        resultPtr->calib_data_set_ptr = dataAdapter.GetCalibDataSetPtr();
        resultPtr->image_source_ptr = imageSourcePtr;

        // pointcloud
        {
          if (nullptr != pclBufferPtr) {
            if (!pclBufferPtr->IsEmpty()) {
              auto latestPair{pclBufferPtr->GetLatest()};
              if (latestPair.second == nullptr) {
                std::cout << "[" << to_yellow("Warr") << "]"
                          << "latest point cloud nullptr" << std::endl;
                break;
              }
              std::uint32_t index{latestPair.first};
              PointCloudSourcePtr pclSourcePtr{latestPair.second};

              while (true) {
                if (pclSourcePtr == nullptr) {
                  break;
                }
                if (pclSourcePtr->timestamp == currentTimestamp) {
                  resultPtr->pcl_source_ptr = pclSourcePtr;
                  break;
                } else {
                  if (pclBufferPtr->HasPrev(index)) {
                    index = pclBufferPtr->GetPrevIndex(index);
                    pclSourcePtr = pclBufferPtr->Get(index);
                  } else {
                    break;
                  }
                }
              }
            } else {
              std::cout << "[" << to_yellow("Warr") << "]"
                        << " point cloud is empty ..." << std::endl;
            }
          }
        }

        // infer
        {
          Timer tInferDecode;
          e2eProcessorPtr->msleep(50);
          e2eProcessorPtr->e2e_net_.Run(*resultPtr);
          std::cout << "[Info] E2E net Infer+Decode Run Time: "
                    << std::to_string(tInferDecode.elapsed() / (float)1000)
                    << " ms, " << std::endl;

          resultPtr->timestamp = currentTimestamp;
        }

        e2eProcessorPtr->result_buffer_ptr_->Append();

        // visualization
        vis.Enqueue(*resultPtr);

      } catch (const std::exception& e) {
        std::cout << "[" << to_red("Error") << "]"
                  << "E2EProcessor routine run error, " << e.what()
                  << std::endl;
      }
    }

  } catch (const std::exception& e) {
    std::cout << "[" << to_red("Error") << "]" << "E2EProcessor run error, "
              << e.what() << std::endl;
  } catch (...) {
    std::cout << "[" << to_red("Error") << "]" << "E2EProcessor run error, "
              << "Unexpected" << std::endl;
  }
}

/// @brief run
void E2EProcessor::Stop() noexcept {
  std::lock_guard<std::mutex> l(lock_);
  if (IsRun()) {
    cmd_stop_ = true;
    if (task_thread_.joinable()) {
      task_thread_.join();
    }
    e2e_net_.Close();
    is_run_ = false;
  }
}

}  // namespace perception
}  // namespace autodrive